#include "SERIALAPI.h"
#include "mouse_sensor.h"
#include "lpc_types.h"
#include "math.h"
#define DEBUG                   0x01
#if DEBUG
#include "debug_frmwrk.h"
#endif

#define THRESHOLD               1
#define SPEED                   32
#define MIN_SPEED               0x10
#define MAX_SPEED               0x27
#define MULTIPLIER              (1.3f)
#define READING_X_90_POS        3077
#define READING_Y_90_POS        -5287
#define READING_X_90_NEG        0
#define READING_Y_90_NEG        0
#define ROT_SPEED               25
#define STEP                    0x01
#define DISTANCE                0x32 // = 50(dec)


//TODO READINGS mx_forward xy_reverse



void move_straight_test(uint8_t dist)
{
    uint32_t t = 10000000;
    reset_mouse_position();
    MOVE_FORWARD(SPEED);
    uint8_t m1 = SPEED, m2 = SPEED;
    
    while(get_x_cm() < dist)
    {
        while(--t);
        t = 50;
        //while(!read_front_sensor());  
        if(get_y_cm() < -THRESHOLD)
        {
            m1++;
            m2--;
        }
        else if(get_y_cm() > THRESHOLD) 
        {
            m2++;
            m1--;
        }
        else
        {
            m1 = SPEED;
            m2 = SPEED;
        }
        if (m1 > MAX_SPEED)
            m1 = MAX_SPEED;
        else if (m1 < MIN_SPEED)
            m1 = MIN_SPEED;
        if (m2 > MAX_SPEED)
            m2 = MAX_SPEED;
        else if (m2 < MIN_SPEED)
            m2 = MIN_SPEED;
        M1_FORWARD(m1);
        M2_FORWARD(m2);
        #if DEBUG
        int32_t x = get_x(), y = get_y();
        int32_t fi = get_fi();
        if(x >= 0)
        { 
            UARTPutDec32(LPC_UART0, x);
        }
        else
        {
            UARTPuts(LPC_UART0, "-");
            UARTPutDec32(LPC_UART0, -x + 1);
        }
        UARTPuts(LPC_UART0, " : ");
        if(y >= 0)
        { 
            UARTPutDec32(LPC_UART0, y);
        }
        else
        {
            UARTPuts(LPC_UART0, "-");
            UARTPutDec32(LPC_UART0, -y + 1);
        }
        UARTPuts(LPC_UART0, " : ");
        if(fi >= 0)
        { 
            UARTPutDec32(LPC_UART0, fi);
        }
        else
        {
            UARTPuts(LPC_UART0, "-");
            UARTPutDec32(LPC_UART0, -fi + 1);
        }
        UARTPuts(LPC_UART0, "\r\n");
        #endif
        
    }
    
    STOP_MOTORS();
    return;
}

/*
void wallfollow_test()
{
    //call wallfollowing
}

void linefollow_test()
{
    //call line following
}

void dead_reckon(uint8_t x, uint8_t y)
{
    double angle = atan2(y,x);
    M1_FORWARD(30);EPS
    while(get_angle() < angle);
    MOVE_FORWARD(30);
    while((get_x()) < x);
    STOP_MOTORS();
}
void position_test()EPS
{
    dead_reckon(800,200);
}



void final_test(void)
{
    LINE_AutoCalibrate(void);
    //follow till end
    move_straight();        // till line; stop motors
    //Follow line - set distance to (200,0);
    //follow wall till line
    //follow line - set distance to (400,0);
    //reset motors to SPEED
    M1_INCREASE(uint8_t(SPEED*MULTIPLIER));
    //MOVE_CURVE((uint8_t)(SPEED*MULTIPLIER), SPEED);
    while(read_sensor_distance(RF_SENSOR) < 8 ||
         read_sensor_distance(RF_SENSOR) > 35);
    //follow wall till line
    //follow line - set distance to (800, 200);
    move_straight();
    //Follow line     
}
*/

/*

int32_t abs__(int32_t val)
{
    return (val > 0 ? val : -val);
}

void turn_test(uint8_t direction)    // Assuming 90
{
    int32_t orig_x = get_x();
    int32_t orig_y = get_y();
    if(direction > 0)
    {
       M1_FORWARD(ROT_SPEED);
       M2_REVERSE(ROT_SPEED);
       while(abs__(get_x() - orig_x) < READING_X_90_POS &&
            abs__(get_y() - orig_y) < READING_Y_90_POS)
            {
                UARTPutDec32(LPC_UART0, get_x());
                UARTPuts(LPC_UART0, ":");
                UARTPutDec32(LPC_UART0, get_y());
                UARTPuts_(LPC_UART0, "");
            };
        M1_STOP();
    }
    else if(direction < 0)
    {
        M2_FORWARD(ROT_SPEED);
        M1_REVERSE(ROT_SPEED);
        while(abs__(get_x() - orig_x) < READING_X_90_NEG &&
            abs(get_y() - orig_y) < abs(READING_Y_90_NEG));
        M2_STOP(); 
    }
}



void reading_test(void)
{    
    int32_t x;
    int32_t y;
    uint32_t time;
    while (1)
    {
        x = get_x();
        y = get_y();
        time = 10000;
        if(x >=0)
        { 
            UARTPutDec32(LPC_UART0, x);
        }
        else
        {
            UARTPuts(LPC_UART0, "-");
            UARTPutDec32(LPC_UART0, -x + 1);
        } 
        UARTPuts(LPC_UART0, " : ");
        if(y >= 0)
        { 
            UARTPutDec32(LPC_UART0, y);
        }
        else
        {
            UARTPuts(LPC_UART0, "-");
            UARTPutDec32(LPC_UART0, -y + 1);
        }
        UARTPuts(LPC_UART0, "\r\n");
        while (time--); 
    }
}
*/

void rot_test(void)
{
    int32_t x = get_x();
    int32_t y = get_y();
    
    M1_FORWARD(ROT_SPEED);
    M2_REVERSE(ROT_SPEED);
    //while (get_x() - x < desired_x && get_y() - y < desired_y);
    uint32_t time = 6000000;        // rot time [5000000-900000]
    while (time--);
    M1_STOP();
    M2_STOP();
    x = get_x() - x;
    y = get_y() - y;
    while (1)
    {
        time = 10000;               // print time
        if(x >= 0)
        { 
            UARTPutDec32(LPC_UART0, x);
        }
        else
        {
            UARTPuts(LPC_UART0, "-");
            UARTPutDec32(LPC_UART0, -x + 1);
        } 
        UARTPuts(LPC_UART0, " : ");
        if(y >= 0)
        { 
            UARTPutDec32(LPC_UART0, y);
        }
        else
        {
            UARTPuts(LPC_UART0, "-");
            UARTPutDec32(LPC_UART0, -y + 1);
        }
        while (time--); 
    }
}

int main(void)
{
    #if DEBUG
    debug_frmwrk_init();
    #endif
    init_mouse_sensors();
    INITIALISE();
    UARTPuts_(LPC_UART0, "Ver 1.07\n\rWaiting for the mouse to respond");
    while (!get_mouse_active());
    UARTPuts_(LPC_UART0, "Mouse found. Starting doing stuff\n\r");
    //rot_test();
    move_straight_test(DISTANCE);
    
    return;
}

